//
// Created by az on 2020/6/16.
//
#include "waypoint_msgs.h"

ros::Publisher wp_pub_;

nav_msgs::Path waypoints;
std::string waypoint_type = std::string("manual");

void publish_waypoints() {
    waypoints.header.frame_id = std::string("AUTO");
    waypoints.header.stamp = ros::Time::now();
    wp_pub_.publish(waypoints);
}

void goal_callback(const geometry_msgs::PoseStamped::ConstPtr& msg){
    if(waypoint_type == std::string("manual")){
        if(msg->pose.position.z >= 0){
        // if height > 0, it's a valid goal;
            geometry_msgs::PoseStamped pt = *msg;

            waypoints.poses.clear();
            waypoints.poses.push_back(pt);
            publish_waypoints();
        } else {
            ROS_WARN("[waypoint_generator] invalid goal in manual mode.");
        }
    }
}

void target_from_Pad_callback(const geometry_msgs::PoseStamped::ConstPtr& msg){
    if(waypoint_type != std::string("manual")) return;

    if(msg->pose.position.z >= 0){
        geometry_msgs::PoseStamped pt = *msg;

        if(pt.header.frame_id == "HANDLE"){
            waypoints.poses.clear();
            waypoints.poses.push_back(pt);

            waypoints.header.frame_id = "HANDLE";
            waypoints.header.stamp    = pt.header.stamp;
            wp_pub_.publish(waypoints);
        }
        else if(pt.header.frame_id == "AUTO"){
            waypoints.poses.clear();
            waypoints.poses.push_back(pt);

            waypoints.header.frame_id = "AUTO";
            waypoints.header.stamp    = pt.header.stamp;
            wp_pub_.publish(waypoints);
        }

        ROS_INFO("[waypoint_generator node]: target point from Pad !");
    }
}

int main(int argc, char **argv){
    ros::init(argc, argv, "waypoint");
    ros::NodeHandle n("~");
    n.param("waypoint_type", waypoint_type, std::string("auto"));

    ros::Subscriber goal_sub = n.subscribe("/move_base_simple/goal", 10, goal_callback);
    ros::Subscriber goal_from_Pad_sub = n.subscribe
            ("/target_from_Pad", 10, target_from_Pad_callback);
    wp_pub_ = n.advertise<nav_msgs::Path>("/waypoints", 10);

    ros::spin();
    return 0;
}

